/***************************************************************************************************************************
* opencv_testcam_frame.cpp
*
* Author: SFL
*
* Update Time: 2019.4.25
*
* Introduction:  视觉识别节点，用来识别红色框
***************************************************************************************************************************/

#include <ros/ros.h>
#include <math.h>
#include <time.h>
#include<image_transport/image_transport.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/core/core.hpp> 
#include<opencv2/imgproc/imgproc.hpp>  
#include "iostream"
#include<fsd_common_msgs/img_pro_info.h>
#include <typeinfo>
//static const std::string OPENCV_WINDOW = "Image window";
using namespace std;
using namespace cv;
long int t_last = cv::getTickCount(),t_now = cv::getTickCount();
long double t_during = 0.0;
int red_offset_add ;
int red_offset_sub ;
int red_offset_line ;
int alpha_slider_add;
int alpha_slider_sub;
int alpha_slider_line;
int picture_width = 1280;
int picture_height = 800;
//存储前一帧的轮廓信息
fsd_common_msgs::img_pro_info info_pub_last;
fsd_common_msgs::img_pro_info info_pub;
//按照轮廓周长排序
// static inline bool LinesSortFun(vector<cv::Point> contour1,vector<cv::Point> contour2)  
// {  
//     return (contour1.size() > contour2.size());  
// }  
//按照轮廓面积排序  由大到小
static inline bool ContoursSortFun(vector<cv::Point> contour1,vector<cv::Point> contour2)  
{  
    return (cv::contourArea(contour1) > cv::contourArea(contour2));  
}
// static inline bool ContoursSortFun(vector<cv::Point> contour1,vector<cv::Point> contour2)  
// {  
//     return contour1.size() > contour2.size();
// }

static inline bool x_order(Point contours1,Point contours2)
{
    return(contours1.x < contours2.x);
}
static inline bool y_order(Point contours1,Point contours2)
{
    return(contours1.y < contours2.y);
}
// void Point_order(const vector<vector<Point> > &contours_ploy)
// {
//     int size = contours_ploy.size();
//     std::sort(contours_ploy[0].begin(),contours_ploy[0].end(),x_order); 
// }
void sort_Rectangle(vector<Point> contours_ploy_out,vector<Point> &contours_max)
{//这个函数的目的是求矩形四个角的坐标？求最大轮廓的bounding box矩形包围框四个角的坐标（最外面的框不一定是矩形，斜看是梯形）
    //0 是左上角，1是左下角，2是右上角，3是右下角
    //索引为0的轮廓按照x从小到大排序
    std::sort(contours_ploy_out.begin(),contours_ploy_out.end(),x_order);
    contours_max[0].x = contours_ploy_out.at(0).x;
    contours_max[1].x = contours_ploy_out.at(0).x;
    contours_max[2].x = contours_ploy_out.back().x;
    contours_max[3].x = contours_ploy_out.back().x;
    std::sort(contours_ploy_out.begin(),contours_ploy_out.end(),y_order);
    contours_max[0].y = contours_ploy_out.at(0).y;
    contours_max[1].y = contours_ploy_out.back().y;
    contours_max[2].y = contours_ploy_out.at(0).y;
    contours_max[3].y = contours_ploy_out.back().y;
    //这下面是东升写的，暂时完全不知所云
    // if((contours_ploy[0][0].y < contours_ploy[0][1].y))
    // {
    //     contours_max[0] = Point(contours_ploy[0][0].x,contours_ploy[0][0].y);
    //     contours_max[1] = Point(contours_ploy[0][1].x,contours_ploy[0][1].y);
    // }
    // else 
    // {
    //     contours_max[0] = Point(contours_ploy[0][1].x,contours_ploy[0][1].y);
    //     contours_max[1] = Point(contours_ploy[0][0].x,contours_ploy[0][0].y);
    // }
    // if(contours_ploy[0][2].y < contours_ploy[0][3].y)
    // {
    //     contours_max[2] = Point(contours_ploy[0][2].x,contours_ploy[0][2].y);
    //     contours_max[3] = Point(contours_ploy[0][3].x,contours_ploy[0][3].y);
    // }
    // else
    // {
    //     contours_max[2] = Point(contours_ploy[0][3].x,contours_ploy[0][3].y);
    //     contours_max[3] = Point(contours_ploy[0][2].x,contours_ploy[0][2].y);
    // }

}
int caculate_height_difference(vector<Point> contours_ploy_out)//本函数暂时还不稳定，再讨论讨论决定是否使用吧
{
    int height_difference;
    int x_min = picture_width;
    int x_max = 0;
    int y_left_min = picture_height;
    int y_left_max = 0;
    int y_right_min = picture_height;
    int y_right_max = 0;
    // int i_left = 0;
    // int i_right = 0;
    for (int i = 0; i < contours_ploy_out.size(); i++)
    {
        if(contours_ploy_out[i].x > x_max)
        {
            x_max = contours_ploy_out[i].x;
            // i_right = i;
        }
        if(contours_ploy_out[i].x < x_min)
        {
            x_min = contours_ploy_out[i].x;
            // i_left = i;
        }
    }
    for (int i = 0; i < contours_ploy_out.size(); i++)
    {
        if(contours_ploy_out[i].x > x_max - 40)//方框右边缘的点认为都在最右的点左侧10个像素以内，如果参数大小不合适可以再改
        {
            if(contours_ploy_out[i].y > y_right_max)//找到右边缘最下面的点
            {
                y_right_max = contours_ploy_out[i].y;
            }
            if(contours_ploy_out[i].y < y_right_min)//找到右边缘最上面的点
            {
                y_right_min = contours_ploy_out[i].y;
            }
        }
        if(contours_ploy_out[i].x < x_min +40)//方框左边缘的点认为都在最左的点右侧10个像素以内，如果参数大小不合适可以再改
        {
            if(contours_ploy_out[i].y > y_left_max)//找到左边缘最下面的点
            {
                y_left_max = contours_ploy_out[i].y;
            }
            if(contours_ploy_out[i].y < y_left_min)//找到左边缘最上面的点
            {
                y_left_min = contours_ploy_out[i].y;
            }
        }
    }
    int left_height = y_left_max - y_left_min;
    int right_height = y_right_max - y_right_min;
    height_difference = right_height - left_height;

    return height_difference;
}

void red_Filters(Mat img_hsv,Mat &img_red)
{
    int red_hsv = 120;
    int red_offset = 10;
    int s_hsv = 30;
    //红色区域筛选
    for (int i = 0; i < img_hsv.rows; i++)
    {
        for (int j = 0; j < img_hsv.cols; j++)
        {
            //CvScalar hsv_point = cvGet2D(img_hsv, i, j);//获取像素点为（j, i）点的HSV的值 
            Vec3i hsv_point = img_hsv.at<Vec3b>(i,j);
            if ((hsv_point.val[0]>red_hsv - red_offset)&&(hsv_point.val[0]<red_hsv + red_offset) && (hsv_point.val[1] > s_hsv))
            {
                //cvSet2D(img_red, i ,j, white_point);
                img_red.at<uchar>(i,j) = 255;
            }
        }           
    }

}
void similar_Decision()
{
    //通过相似度查找符合条件的轮廓
    //cout<<"第一种相似判断，结果两个轮廓之间的相似度为：  "<<matchShapes(contours_ploy[0],contours_ploy[0],CV_CONTOURS_MATCH_I1, 0)<<endl;
    //cout<<"第二种相似判断，结果两个轮廓之间的相似度为：  "<<matchShapes(contours_ploy[0],contours_ploy[1],CV_CONTOURS_MATCH_I2, 0)<<endl;
    //cout<<"第三种相似判断，结果两个轮廓之间的相似度为：  "<<matchShapes(contours_ploy[0],contours_ploy[1],CV_CONTOURS_MATCH_I3, 0)<<endl;
}
void center_Compute(vector<vector<Point> > contours_ploy_filter,vector<int > contours_id_filter,vector<Moments> &mu,vector<Point2f> &mc)
{
    int num = contours_id_filter.size();
    for(size_t i = 0; i < num;i++)
    {
        mu[i] = moments( contours_ploy_filter[contours_id_filter[i]], true ); 
        mc[i] = Point2d( mu[i].m10/mu[i].m00 , mu[i].m01/mu[i].m00 );
        cout<<mc[i].x<<"     "<<mc[i].y<<endl;
    }
}
void angel_Compute()
{
    //计算方向
    // double a = mu[0].m20 / mu[0].m00 - mc[0].x*mc[0].x;
    // double b = mu[0].m11 / mu[0].m00 - mc[0].x*mc[0].y;
    // double c = mu[0].m02 / mu[0].m00 - mc[0].y*mc[0].y;
    // double theta = fastAtan2(2*b,(a - c))/2;//此为形状的方向
    //cout<<"角度 = "<<theta<<endl;
}
void double_Line(vector<Point2f> mc,vector<int > &contours_id)
{
    //判断哪两个轮廓中心点之间的距离最小，将这两个轮廓的id保存在contours_id中
    //我们认为内外框的中心点应该距离是非常近的
    cout<<"double_Line arrived!"<<endl;
    int lenth = mc.size();
    double dis_min = fabs(mc[0].x-mc[1].x) + fabs(mc[0].y-mc[1].y);
    double dis_now = 0;
    contours_id.push_back(0);
    contours_id.push_back(1);
    for(int i = 0;i < lenth-1;i++)
    {
        for(int j = i + 1;j < lenth;j++)
        {
            dis_now = fabs(mc[i].x-mc[j].x) + fabs(mc[i].y-mc[j].y);
            if(dis_now < dis_min)
            {
                dis_min = dis_now;
                contours_id[0] = i;
                contours_id[1] = j;      
            }  
        }
    }
    cout<<"dis_min = "<<dis_min<<endl;
    cout<<"dis_min_real = "<<fabs(mc[contours_id[0]].x-mc[contours_id[1]].x) + fabs(mc[contours_id[0]].y-mc[contours_id[1]].y)<<endl;
}
bool contours_Out_Decision(vector<Point>  contours_ploy)
{
    for(int j = 0;j < contours_ploy.size();j++)
    {
        if( (contours_ploy[j].x < 20 || contours_ploy[j].x > picture_width-20)  || (contours_ploy[j].y < 20 || contours_ploy[j].y > picture_height-20) )
        {
            return (1);
        }
    }
    return (0);
}
void draw_Circle(vector<Point2f> mc,Mat &img)
{
    int lenth = mc.size();
    Point2d center;
    for(int i = 0;i <lenth;i++)
    {
        center = Point(floor(mc[i].x),floor(mc[i].y));
        cv::circle(img, center, 5, Scalar(0,0,0),2);
    }  
}
void approxPolyDP_Compute(vector<vector<Point> > contours,vector<vector<Point> > &contours_ploy)
{
     for (size_t i = 0; i< contours.size(); i++)
    {
        //将轮廓做多边形逼近
        approxPolyDP(contours[i], contours_ploy[i], 3, true);//第三个变量表征逼近的程度
        //printf("%s\n",typeid(contours[0]).name());//类型
        //cout<<"类型为"<<typeid(contours[0]).name()<<endl;

        //查找最小的矩形包围框
        //minRect[i] = minAreaRect( Mat(contours_ploy[i]) );
        //rects_ploy[i] = boundingRect(contours_ploy[i]);    
        // if (contours_ploy[i].size() >5)
        // {
        //     RotatedRect temp1 = minAreaRect(contours_ploy[i]);
        //     RotatedRect_ploy.push_back(temp1);
        //     // 椭圆
        //     // RotatedRect temp2 = fitEllipse(contours_ploy[i]);
        //     // ellipse_ploy.push_back(temp2);
        // }
    }
}
void contours_Filters(vector<vector<Point> > contours_ploy,vector<int > &contours_id)
{
    //判断逼近后的多边形是大于四个边的并且面积大于一定的值
    int lenth = contours_ploy.size();
    //cout<<"lenth = "<<lenth<<endl;
    for(size_t i = 0;i < lenth;i++)
    {
        if(contours_ploy[i].size() >= 4  && cv::contourArea(contours_ploy[i]) > 1600)
        {
            contours_id.push_back(i);   
        }
    }
}
class ImageConverter
{
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    // image_transport::Publisher image_pub_;
    ros::Publisher contours_pub_ ;
    string image_topic;

    
public:
    ImageConverter()
    : it_(nh_)
    {
        nh_.param<string>("/use_opencv_find_box/image_topic", image_topic, std::string("/iris/usb_cam/image_raw"));
        
        // Subscrive to input video feed and publish output video feed
        image_sub_ = it_.subscribe(image_topic, 1,&ImageConverter::imageCb, this);
        // image_pub_ = it_.advertise("/image_converter/output_video", 1);
        //contours_pub_ = it_.advertise("/contours_topic", 1);
        contours_pub_ = nh_.advertise<fsd_common_msgs::img_pro_info>("/img_pro_info", 50);
        //cv::namedWindow(OPENCV_WINDOW);
    }
 
    ~ImageConverter()
    {
        //cv::destroyWindow(OPENCV_WINDOW);
    }
    
    void imageCb(const sensor_msgs::ImageConstPtr& msg)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
        }
        // 矩阵访问
        //如果是采用Mat形式存储，想要访问灰度图像的灰度值，可以采用如下方法：
        //int value = img.at<uchar>(i,j)[k];//k表示通道数


        // 画圈
        // if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
        // {
        //     cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
        // }

        t_now = cv::getTickCount();
        t_during = (t_now-t_last)/cv::getTickFrequency();
        // std::cout <<"Time of image = "<< t_during<<endl;
        t_last = t_now;

        //载入图片
        //cv::Mat img = imread("momo3.jpeg",1);
        //变量定义
        cv::Mat img_gauss,img_gray,img_hsv,img_binary,img_red,img_red_gauss,img_red_median;
        //白色的点，用于赋值
        // CvScalar white_point;
        // white_point.val[0] = 255;
        // white_point.val[1] = 255;
        // white_point.val[2] = 255;
        cv::Mat img = cv_ptr->image;
        // cv::resize(img, img, Size(640, 360));
        
        img_red = Mat::zeros(img.size(),CV_8UC1);
	    cv::GaussianBlur(img,img_gauss,cv::Size(3,3),0);
        cvtColor(img_gauss,img_hsv,CV_RGB2HSV);
        // cvtColor(img_hsv,img_gray,CV_RGB2GRAY);
        red_Filters(img_hsv,img_red);
        //求取灰度均值
        // cv:Scalar gray_Val = cv::mean(img_gray);
        // float gray_Mean_0 = gray_Val.val[0];
        // std::cout<<"gray mean value = "<<gray_Mean_0<<std::endl;
        //某一点赋值
        // img.at<Vec3b>(200,100)[0] = 120;
        // img.at<Vec3b>(200,100)[1] = 255;
        // img.at<Vec3b>(200,100)[2] = 240;
        // Vec3i hsv_point = img_hsv.at<Vec3b>(122,100);
        // cout<<"hsv0 = "<<hsv_point.val[0]<<endl;
        // cout<<"hsv1 = "<<hsv_point.val[1]<<endl;
        // cout<<"hsv2 = "<<hsv_point.val[2]<<endl;
        //Vec3i rgb_point = img.at<Vec3b>(200,100);
        /////////////////////////////////////////////////////////////////////////////////
        //二值化处理
        //int thresh = 120;
        //int maxValue = 250;
        //threshold(img_gray,img_binary, thresh, maxValue, THRESH_BINARY);
        /////////////////////////////////////////////////////////////////////////////////
        //canny算子
        // Mat canny_img;
	    // cv::Canny(img_gray,canny_img,120,250);
        //imshow("CannyImg", canny_img);
        /////////////////////////////////////////////////////////////////////////////////
        //存储轮廓
        vector<vector<Point> > contours;
	    vector<Vec4i> hierarchy;
        //中值滤波
        cv::medianBlur(img_red,img_red_median,11);
        //高斯滤波
        //cv::GaussianBlur(img_red_gauss,img_red_median,cv::Size(3,3),0);
        //查找轮廓
        findContours(img_red_median,contours,hierarchy,RETR_LIST,CHAIN_APPROX_NONE,Point());
        //findContours(img_red_median,contours,hierarchy,RETR_LIST,CHAIN_APPROX_TC89_L1,Point());

	    // Mat imageContours = Mat::zeros(img.size(),CV_8UC1);
        // Mat poly_image = Mat::zeros(img.size(),CV_8UC1); //凸包图像
	    // Mat Contours = Mat::zeros(img.size(),CV_8UC1);  //绘制

        // TODO:轮廓排序
        // std::sort(contours.begin(),contours.end(),ContoursSortFun); 
        //把轮廓按照面积从大到小掉顺序排序并没有理解有什么意义，暂时注释掉，未来需要掉话再更改

        //cout<<"第一种相似判断，结果两个轮廓之间的相似度为：  "<<matchShapes(contours[0],contours[1],CV_CONTOURS_MATCH_I1, 0)<<endl;
        // 定义逼近后的存储容器
        vector<vector<Point> > contours_ploy(contours.size());
        //vector<Rect> rects_ploy(contours.size());
        //vector<RotatedRect> RotatedRect_ploy;//注意：由于下面赋值的过程中有个点数大于5的条件，所以这里没有直接初始化，才有下面pushback的方法添加值。
        //vector<RotatedRect> minRect( contours_ploy.size() );
        approxPolyDP_Compute(contours,contours_ploy);//多边形逼近
        ///////////////////////////////////////////////////////////////////////
        //筛选之后的轮廓信息
        vector<int > contours_id_filter;
        contours_Filters(contours_ploy,contours_id_filter);//边数超过4,面积大于1600的轮廓的id提取出来
        cout << "滤波后的轮廓数量为：" << contours_id_filter.size() << endl;
        //////////////////////////////////////////////////////////////////////
        //零阶原点距和一阶原点距
        vector<Moments> mu(contours_id_filter.size()); 
        vector<Point2f> mc(contours_id_filter.size());
        center_Compute(contours_ploy,contours_id_filter,mu,mc);//计算每一个轮廓的中心点，保存在mc
        //////////////////////////////////////////////////////////////////////
        //draw_Circle(mc,img);
        vector<int > contours_id;//contours_id[0]是内边框的索引的索引，contours_id[1]是外边框的索引的索引。
        //中心点保存
        int filter_flag = 0;
        Point2d Center,Center_0,Center_1;//保存内外框的中心点和总的中心点
        if(contours_id_filter.size() > 1)
        {
            double_Line(mc,contours_id);//判断哪两个轮廓的中心点最近，返回这两个轮廓的id
            filter_flag = 1;
            Center_0 = Point(floor(mc[contours_id[0]].x),floor(mc[contours_id[0]].y));
            Center_1 = Point(floor(mc[contours_id[1]].x),floor(mc[contours_id[1]].y));
            if((fabs(Center_0.x-Center_1.x) + fabs(Center_0.y-Center_1.y)) > 50)
            {//判断如果两中心点距离太远的话就认为这俩轮廓不是框的轮廓
                cout<<"dis_min_out = "<<fabs(mc[contours_id[0]].x-mc[contours_id[1]].x) + fabs(mc[contours_id[0]].y-mc[contours_id[1]].y)<<endl;
            }
            //contours_id_filter[contours_id[0]]是内边框的索引，contours_id_filter[contours_id[1]]是外边框的索引。
        }
        else
        {
            ;
        }
        if(filter_flag == 1 && (fabs(Center_0.x-Center_1.x) + fabs(Center_0.y-Center_1.y)) < 50 )//这块注意一下，会不会出现center未赋值的情况
        {
            cout<<"already in the condition ............................"<<endl;
            //如果两个轮廓中心点之间的距离符合要求的话，取其均值作为框的中心点
            Center = Point(floor(0.5*mc[contours_id[0]].x + 0.5*mc[contours_id[1]].x),floor(0.5*mc[contours_id[0]].y + 0.5*mc[contours_id[1]].y));
            //保存面积最大的轮廓信息
            vector<Point> contours_max(4);
            
            //用于判断轮廓是否靠近视野边界了
            bool out_flag = contours_Out_Decision(contours_ploy[contours_id_filter[contours_id[0]]]); 
            info_pub.find_box_flag = 1;
            info_pub.x_pos = Center.x - picture_width / 2;
            info_pub.y_pos = picture_height / 2 - Center.y;
            cout<<"id_0 = "<<contours_id[0]<<endl;
            cout<<"id_1 = "<<contours_id[1]<<endl;
            //在图像上显示
            if(out_flag == 0)
            {
                //如果没有超出边界就显示绿色
                drawContours(img,contours_ploy,contours_id_filter[contours_id[0]],Scalar(0,255,0),3,8,hierarchy);
                drawContours(img,contours_ploy,contours_id_filter[contours_id[1]],Scalar(0,255,0),3,8,hierarchy);
                cv::circle(img, Center_0, 5, Scalar(0,0,0),2);
                cv::circle(img, Center_1, 5, Scalar(0,0,0),2);
                cv::circle(img, Center, 7, Scalar(255,255,255),2);
            }
            else
            {
                //过于靠近视野边界就显示蓝色
                drawContours(img,contours_ploy,contours_id_filter[contours_id[0]],Scalar(255,0,0),3,8,hierarchy);
                drawContours(img,contours_ploy,contours_id_filter[contours_id[1]],Scalar(255,0,0),3,8,hierarchy);
                cv::circle(img, Center_0, 5, Scalar(0,0,0),2);
                cv::circle(img, Center_1, 5, Scalar(0,0,0),2);
                cv::circle(img, Center, 7, Scalar(255,255,255),2);
            }
            //计算矩形bounding box的四个角的坐标
            sort_Rectangle(contours_ploy[contours_id_filter[contours_id[1]]],contours_max);
            info_pub.left_edge = contours_max[0].x;
            info_pub.right_edge = contours_max[3].x;
            info_pub.up_edge = contours_max[0].y;
            info_pub.down_edge = contours_max[3].y;
            info_pub.width = info_pub.right_edge - info_pub.left_edge;
            info_pub.height = info_pub.down_edge - info_pub.up_edge;
            cout<<"width = "<<info_pub.width<<"   height = "<<info_pub.height <<endl;
            cout<<"left_edge = "<<info_pub.left_edge<<"   right_edge = "<<info_pub.right_edge <<endl;
            cout<<"up_edge = "<<info_pub.up_edge<<"   down_edge = "<<info_pub.down_edge <<endl;
            info_pub.height_difference = caculate_height_difference(contours_ploy[contours_id_filter[contours_id[1]]]);
            if(info_pub_last.find_box_flag == 0)//上一帧没有框在视野内
            {
                contours_pub_.publish(info_pub);
                info_pub_last = info_pub;
            }
            else
            {//因为x_pos,y_pos是整形数据，这种适用于浮点型的滤波参数不太好调整
                info_pub.x_pos = info_pub_last.x_pos * 0.8 + (1-0.8) * info_pub.x_pos;
                info_pub.y_pos = info_pub_last.y_pos * 0.8 + (1-0.8) * info_pub.y_pos;
                info_pub.height_difference = info_pub_last.height_difference * 0.9 + (1-0.9) * info_pub.height_difference;
                contours_pub_.publish(info_pub);//publish信息
                info_pub_last = info_pub;
            }
        }
        else
        {
            //publish信息
            info_pub.find_box_flag = 0;
            info_pub.dis = -1;
            info_pub.left_edge = 0;
            info_pub.right_edge = 0;
            info_pub.up_edge = 0;
            info_pub.down_edge = 0;
            info_pub.width = 0;
            info_pub.height =0;
            info_pub.height_difference = 0;
            info_pub.x_pos = 0;
            info_pub.y_pos = 0;
            info_pub_last = info_pub;
            contours_pub_.publish(info_pub);
        }
        //绘制出点
        //for(int j=0;j<contours[i].size();j++) 
        //{
            //绘制出contours向量内所有的像素点
            //Point P=Point(contours[i][j].x,contours[i][j].y);
            //Contours.at<uchar>(P)=255;
        //}
        //输出hierarchy向量内容
        //char ch[256];
        //sprintf(ch,"%d",i);
        //string str=ch;
        //cout<<"向量hierarchy的第" <<str<<" 个元素内容为："<<endl<<hierarchy[i]<<endl<<endl;
        //绘制轮廓
        //cout<<"size = "<<contours.size()<<endl;
        //绘制凸包的图像
        //cv::circle(poly_image, Point(300,300), 10, Scalar(255),2);
        //drawContours(poly_image,poly,i,Scalar(255),1,8,hierarchy); 
        // if(contours_ploy.size() >= 3)
        // {
        //     for(int m = 0;m < contours_ploy[0].size();m++)
        //     {
        //         cout << "x = " << contours_ploy[0][m].x << "   y = " << contours_ploy[0][m].y << endl;
        //     }
        //     cout<<endl;
        // }
        //查找竖直方向上的直线
        // vector<Vec4i> Lines;
        // float x_mean = 0;
        // float y_mean = 0;
        // //Point vertical_center;
        // int num_vertical = 0;
        // cv::HoughLinesP(imageContours, Lines, 1, CV_PI/180, red_offset_line,20,8);
        // vector<Point2d> Lines_vertical(Lines.size()*2);
        // for (int i = 0; i < Lines.size(); i++)
	    // {
        //     if(abs(Lines[i][1] - Lines[i][3]) > 10 && atan(abs(Lines[i][1] - Lines[i][3])*1.0/abs(Lines[i][0] - Lines[i][2])) > M_PI*3/8)
        //     {
        //         Lines_vertical[num_vertical] = Point2d(Lines[i][0],Lines[i][1]);
        //         num_vertical++;
        //         Lines_vertical[num_vertical] = Point2d(Lines[i][2],Lines[i][3]);
        //         num_vertical++;
        //         //line(img, Point(Lines[i][0], Lines[i][1]), Point(Lines[i][2], Lines[i][3]), Scalar(0, 255, 0), 4, 8);
        //     }
	    // }
        //vertical_center = Point2d(floor(x_mean/num_vertical),floor(y_mean/num_vertical));

        //cout<<"直线条数 = "<<Lines.size()<<endl;     
        // //定义最终绘图的图片
        //Mat draw_rotateRect(img.size(), img.type(), Scalar::all(0));
        // //绘图圆形、矩形
        //RNG rng(12345);
        // //绘图椭圆形、旋转矩形
        // Point2f pot[4];
        // for (size_t i = 0; i<RotatedRect_ploy.size(); i++)
        // {
        //     Scalar color = Scalar(rng.uniform(0,255), rng.uniform(0,255), rng.uniform(0,255));
        //     RotatedRect_ploy[i].points(pot);
        //     for(int j=0; j<4; j++)
        //     {
        //         line(draw_rotateRect, pot[j], pot[(j+1)%4], color);
        //     }
        // }


        //imshow("rotateRect",draw_rotateRect);
        //Update GUI Window
        //imshow("Contours Image",imageContours); //轮廓
        //imshow("gray Image",img_gray);
        // imshow("hsv Image",img_hsv);
        // imshow("red Image",img_red);//hsv提取到的红色区域
        // imshow("red Image_gauss",img_red_median);//高斯滤波后的红色区域
        //imshow("binary Image",img_binary);
        //imshow("poly Image",poly_image); //凸包的图像
        //imshow("draw_rotateRect", draw_rotateRect); 
	    //imshow("Point of Contours",Contours);   //向量contours内保存的所有轮廓点集
        cv::imshow("OPENCV_WINDOW_color", img);
        
        cv::waitKey(3);

    }
     
};
//调整阈值时使用
void on_trackbar_add( int, void* )
{
    red_offset_add = alpha_slider_add;
}
void on_trackbar_sub( int, void* )
{
    red_offset_sub = alpha_slider_sub;
} 
void on_trackbar_line( int, void* )
{
    red_offset_line = alpha_slider_line;
}    
int main(int argc, char** argv)
{
ros::init(argc, argv, "use_opencv_find_box");
ImageConverter ic;
int alpha_slider_line_max = 100;
int alpha_slider_max = 50;
// namedWindow("Linear Offset_add", 1);
// namedWindow("Linear Offset_sub", 1);
//namedWindow("Linear Offset_line", 1);
// createTrackbar( "Trackbar", "Linear Offset_add", &alpha_slider_add, alpha_slider_max, on_trackbar_add );
// createTrackbar( "Trackbar", "Linear Offset_sub", &alpha_slider_sub, alpha_slider_max, on_trackbar_sub );
//createTrackbar( "Trackbar", "Linear Offset_line", &alpha_slider_line, alpha_slider_line_max, on_trackbar_line );
ros::spin();
return 0;
}

